package adv.i31.robot;

import android.opengl.GLES20;
import android.opengl.Matrix;

public class Joint implements IDrawableObject {

	// Opengl matrices are Column-major. Therefore they look transposed. Doesn't
	// matter for identity though

	float[] mAxle = { 0.0f, 0.0f, 1.0f, 1.0f }; // new float[4];

	float mAngle = 0.0f; // -45.0f;

	float mLength = 1.0f; // Distance to next Joint

	float[] mModel = new float[16];
	float[] mScale = new float[16];

	private VertexBuffer mVertexBuffer;

	public Joint() {
		// initialize the vertex buffer, normalized vector pointing up
		mVertexBuffer = new VertexBuffer(new float[] { 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f });
	}

	public void draw(Program p, Camera c) {
		GLES20.glVertexAttribPointer(p.getmPositionHandle(), mVertexBuffer.getmPositionDataSize(),
				GLES20.GL_FLOAT, false, mVertexBuffer.getmStrideBytes(),
				mVertexBuffer.getmFloatBuffer());

		GLES20.glEnableVertexAttribArray(p.getmPositionHandle());

		float[] mVPMatrix = new float[16];
		float[] mMVPMatrix = new float[16];

		Matrix.setIdentityM(mScale, 0);
		Matrix.scaleM(mScale, 0, 0.0f, mLength, 0.0f);
		Matrix.multiplyMM(mModel, 0, mTransform, 0, mScale, 0);

		// calculate View-Projection Matrix
		Matrix.multiplyMM(mVPMatrix, 0, c.getmProjMatrix(), 0, c.getmViewMatrix(), 0);

		// calculate MODEL-View-Projection Matrix
		Matrix.multiplyMM(mMVPMatrix, 0, mVPMatrix, 0, mModel, 0);

		// Pass the MVP Matrix to the shader
		GLES20.glUniformMatrix4fv(p.getmMVPMatrixHandle(), 1, false, mMVPMatrix, 0);

		GLES20.glDrawArrays(GLES20.GL_LINE_STRIP, 0, mVertexBuffer.getNumVertices());
	}

	Joint mPreviousJoint;

	float[] mTransform = new float[16];
	float[] mRotation = new float[16];
	float[] mTranslation = new float[16];

	private final float[] prevRT = new float[16];

	public void calculateTransformation() {

		// every vertex in this joints coordinate system needs to be transformed
		// to the coordinate system of the root joint. This is achieved by
		// successively
		// transforming the vertex coordinates through the coordinate systems of
		// every joint
		// down to the root joint.

		// Transform(n) := R(0) * T(0) * R(1) * T(1) * ... * R(n-1) * T(n-1) *
		// R(n)
		// R : Rotation Matrix
		// T : TransLATION Matrix

		Matrix.setIdentityM(mRotation, 0);
		Matrix.rotateM(mRotation, 0, mAngle, mAxle[0], mAxle[1], mAxle[2]);

		Matrix.setIdentityM(mTranslation, 0);
		Matrix.translateM(mTranslation, 0, 0.0f, mLength, 0.0f);

		if (mPreviousJoint != null) {
			mPreviousJoint.calculateTransformation();
			Matrix.multiplyMM(prevRT, 0, mPreviousJoint.mTransform, 0, mPreviousJoint.mTranslation,
					0);
			Matrix.multiplyMM(mTransform, 0, prevRT, 0, mRotation, 0);
		} else {
			// recursion reached the root joint
			mTransform = mRotation.clone();
		}
	}

}